/**
 * Snowflake MEGA sumo robot test nr.1 - turn left or right  using delays
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "test1.h"
#include "keyboard.h"
#include "motors.h"


void test1_app(void)
{
	struct keyboard_event input;

	// set speed for the test
	int8_t speed = 100;
	bool first_move_dir_left = true;

	// show select info
	gfx_mono_draw_string("Select turn direction", 0, 14, &sysfont);
	gfx_mono_draw_string("ENTER-Left,  UP-Right", 0, 24, &sysfont);

	// Set the first move direction
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			if (input.keycode == KEYBOARD_ENTER) {
				first_move_dir_left = true;
				break;
			} else if (input.keycode == KEYBOARD_UP) {
				first_move_dir_left = false;
				break;
			} else if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				// go back to SYSTEM tests menu
				return;
			}
		}
	}

	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	if (first_move_dir_left) {
		// Show selected move info
		gfx_mono_draw_string("Test 1      turn LEFT", 0, 0, &sysfont);
		gfx_mono_draw_string("       3 sec to start", 0, 24, &sysfont);
	} else {
		// Show selected move info
		gfx_mono_draw_string("Test 1     turn RIGHT", 0, 0, &sysfont);
		gfx_mono_draw_string("       3 sec to start", 0, 24, &sysfont);
	}
		delay_s(1);
		gfx_mono_draw_string("       2 sec to start", 0, 24, &sysfont);
		delay_s(1);
		gfx_mono_draw_string("       1 sec to start", 0, 24, &sysfont);
		delay_s(1);
		gfx_mono_draw_string("       0 sec to start", 0, 24, &sysfont);


	// robot better turns in the left direction so right turn time needs to be bigger
	if (first_move_dir_left) {
		// turn left
		drive( -speed, speed );
		delay_ms(60); // <========== Calibrate this value!!! ===========
	} else {
		// turn right
		drive( speed, -speed );
		delay_ms(50); // <========== Calibrate this value!!! ===========
	}

	drive(0,0); // break motors
	delay_ms(100);
	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
	stop_motors();  // free motors
	return;

}